import threading
import time
from util import *


class LidarSimulation:
    def __init__(self, config_dict: dict):
        self.pcd_points = config_dict['pcd_points']
        self.save_path = config_dict['save_path']
        self.origin_position = config_dict['origin_position']
        self.light_pole_position = config_dict['light_pole_position']
        self.lidar_type = int(config_dict['lidar_type'])
        self.lidar_resolution = float(config_dict['lidar_resolution'])
        self.lidar_range = float(config_dict['lidar_range'])
        self.lidar_list = config_dict['lidar_list']
        self.pcd_level = int(config_dict['pcd_level'])
        self.pole_radius = float(config_dict['pole_radius'])
        self.lidar_height = config_dict['origin_position'][2] - config_dict['light_pole_position'][2]
        self.count = 0
        self.pcd_data = []
        self.filtered_points = None
        self.all_data = None
        self.run_flag = True

    def get_count(self):
        return self.count

    def get_run_flag(self):
        return self.run_flag

    def get_all_data(self):
        return self.all_data

    def run(self):
        start_time = time.time()
        # 数据预处理
        self.pre_data()
        # 点云模拟（多线程）
        if self.filtered_points.shape[0] > 0:
            thread_list = []
            for i in range(self.lidar_type):
                pcd_data_i = self.pcd_data[i]
                thread = threading.Thread(target=self.one_point_filter, args=(i, pcd_data_i,))
                thread.start()
                thread_list.append(thread)
            for thread in thread_list:
                thread.join()
        end_time = time.time()
        print("模拟点云耗时:", end_time - start_time)
        # 点云保存（PCD）
        self.save_pcd()
        self.run_flag = False

    def pre_data(self):
        for m in range(self.lidar_type):
            self.pcd_data.append([])

        if self.pcd_points is not None:
            # 将点云中心移动到原点
            translated_points = self.pcd_points - self.origin_position
            # 计算每个点到原点的距离
            distances = np.linalg.norm(translated_points, axis=1)
            # 假设X轴正方向为水平，Z轴正方向为垂直向上，计算与Z轴的逆时针角度
            angles_with_z_axis = np.degrees(
                np.arctan2(np.sqrt(translated_points[:, 0] ** 2 + translated_points[:, 1] ** 2),
                           translated_points[:, 2]))
            # 新增: 筛选XY平面内角度在指定范围内的点
            angles_with_xy_axis = 90 - np.degrees(np.arctan2(translated_points[:, 1], translated_points[:, 0]))
            if np.any(angles_with_xy_axis < 0):
                angles_with_xy_axis[angles_with_xy_axis < 0] += 360
            # 筛选距离小于等于100米且角度在-45°到0°之间的点
            angle_ = [self.lidar_list[-1] - 3, self.lidar_list[0] + 1]
            angles_with_y_axis = calculate_angle(self.origin_position, self.light_pole_position, self.pole_radius)
            mask_dis = (distances <= self.lidar_range)
            mask_z = (angles_with_z_axis <= (90 - angle_[0])) & (angles_with_z_axis >= (90 - angle_[1]))
            if angles_with_y_axis[0] > angles_with_y_axis[1]:
                mask_xy = (angles_with_xy_axis <= angles_with_y_axis[0]) & (angles_with_xy_axis >= angles_with_y_axis[1])
            else:
                mask_xy = (angles_with_xy_axis <= angles_with_y_axis[0]) | (angles_with_xy_axis >= angles_with_y_axis[1])

            mask = mask_dis & mask_z & mask_xy

            # 应用筛选条件
            self.filtered_points = translated_points[mask]
        else:
            print("No point cloud data provided.")

    def one_point_filter(self, i, pcd_data_i):
        lidar_angle = self.lidar_list[i]
        if i == 0:
            angle_b = self.lidar_list[0] + 0.5
            angle_a = self.lidar_list[1]
        elif i == (self.lidar_type - 1):
            angle_a = self.lidar_list[-1] - 1
            angle_b = self.lidar_list[i - 1]
        elif (self.lidar_list[i - 1] - self.lidar_list[i + 1]) > 0.5:
            angle_a = self.lidar_list[i] - 0.5
            angle_b = self.lidar_list[i] + 0.5
        else:
            angle_a = self.lidar_list[i + 1]
            angle_b = self.lidar_list[i - 1]
        # else:
        #     angle_a = self.lidar_list[i + 1]
        #     angle_b = self.lidar_list[i - 1]
        points_, xy_angle_list = self.filter_points(angle_a, angle_b)

        # 循环发射1800条射线作为一圈点云数据
        j = -1.0 * self.lidar_resolution
        for i in range(int(360 / self.lidar_resolution)):
            j += self.lidar_resolution
            angle_z = np.radians(90 - lidar_angle)
            angle_x = np.radians(j)
            # 计算射线的方向向量
            if lidar_angle == 0:
                direction_vec = np.array([0, 0, 1])
            else:
                direction_vec = calculate_direction_vector(angle_z, angle_x)
            # 获取射线附近角度的点列表
            points_a = xy_angle_list[int(j / self.lidar_resolution)]

            # 根据距离从上述点中选择距离射线最近的点，并保存
            # 检查点集中的点是否满足条件
            if len(points_a) == 0:
                is_point = None
            else:
                # 计算所有点到射线的距离
                distances = np.linalg.norm(points_a, axis=1)
                # 找到最小距离对应的点
                nearest_point_index = np.argmin(distances)
                nearest_point = points_a[nearest_point_index]
                point_z = -nearest_point[2]
                if np.abs(point_z) < (self.lidar_height * 0.5):
                    length = np.dot(nearest_point, direction_vec) / np.linalg.norm(direction_vec)
                else:
                    lidar_angle_a = np.radians(90 + lidar_angle)
                    length = point_z / np.cos(lidar_angle_a)
                if np.abs(length) > self.lidar_range:
                    is_point = None
                else:
                    is_point = np.abs(length) * direction_vec

            if is_point is not None:
                pcd_data_i.append(is_point)
            else:
                pcd_data_i.append(np.array([0, 0, 0]))
        self.count += 1

    def filter_points(self, angle_a, angle_b):
        # 每圈的点数
        num = int(360.0 / self.lidar_resolution)
        # 建立空列表
        xy_angle_list = []
        for m in range(num):
            xy_angle_list.append([])

        # 假设X轴正方向为水平，Z轴正方向为垂直向上，计算与Z轴的逆时针角度
        angles_with_z_axis = np.degrees(np.arctan2(
            np.sqrt(self.filtered_points[:, 0] ** 2 + self.filtered_points[:, 1] ** 2), self.filtered_points[:, 2]
        ))
        angle_ = [angle_a, angle_b]
        mask = (angles_with_z_axis <= (90 - angle_[0])) & (angles_with_z_axis >= (90 - angle_[1]))
        filtered_points = self.filtered_points[mask]

        # 将符合条件的点按照XY平面角度放入列表对应位置
        for i in range(len(filtered_points)):
            if filtered_points[i][0] == 0 and filtered_points[i][1] == 0:
                continue
            else:
                xy_abgle = np.arctan2(filtered_points[i][1], filtered_points[i][0])
                xy_abgle = 90 - np.degrees(xy_abgle)
                if xy_abgle < 0:
                    xy_abgle += 360
                angle = int(xy_abgle / self.lidar_resolution) % num
                if (360 - xy_abgle) <= (self.lidar_resolution / 2):
                    angle = 0
                else:
                    if (xy_abgle / self.lidar_resolution) - int(xy_abgle / self.lidar_resolution) > 0.5:
                        angle += 1
                xy_angle_list[angle].append(filtered_points[i])
        return filtered_points, xy_angle_list

    def save_pcd(self):
        self.all_data = np.concatenate(self.pcd_data, axis=0)
        points2pcd(self.save_path, self.all_data)
        # show_pcd(self.all_data)


